#include "Planner.h"
#include "Prm.h"
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/point_generators_2.h>
#include <boost/make_shared.hpp>
#include <CGAL/Boolean_set_operations_2.h>

using namespace std;

Planner::Planner(Scene* scene) : m_scene(scene)
{
	extract_scenario_from_gui();
}

Planner::~Planner()
{}

/*	This function is invoked by the GUI and is assumed to update the resulting */
void Planner::run()
{
	CollisionDetector* collision_detector = new CollisionDetector(m_robot_polygons, &m_obstacles);
	MultiRobotsSampler* multi_robots_sampler = new MultiRobotsSampler(m_robot_polygons, m_room, collision_detector);

	Prm roadmap(800,
				12,
				collision_detector,
				multi_robots_sampler,
				m_start_confs, m_target_confs);

	roadmap.generate_roadmap();

	roadmap.retrieve_path(&m_path);

	transform_path_for_gui();
}


Point_2	Planner::transformToCGAL(QPointF qp)
{
	// x and y coordinates of the point
	double x = qp.x();
	double y = qp.y();

	Point_2 cpoint(x,y);
	return cpoint;
}
QPointF	Planner::transformToQT(Conf cp)
{
	return QPointF(CGAL::to_double(cp.x()) , CGAL::to_double(cp.y()) );
}

QVector<QPointF> Planner::transformToQT(ConfSet cs)
{
	QVector<QPointF> result;

	foreach(Conf c, cs)
	{
		result.push_back(transformToQT(c));
	}
	return result;
}

QVector<QVector<QPointF> > Planner::transformToQT(list<vector<Conf> > path)
{
	QVector<QVector<QPointF> > result;

	foreach(ConfSet cs, path)
	{
		result.push_back(transformToQT(cs));
	}
	return result;
}

void Planner::transform_path_for_gui()
{
	vector<vector<vector<QConf> > > result_vectors(m_path.size());

	for (int i = 0; i < m_path.size(); i++)
	{
		for (int r = 0; r < m_robot_num; r++)
		{
			vector<QConf> path_for_r;

			QPointF vec;
			double angle = 0;
			if (i != 0)
			{
				QPointF s = transformToQT(m_path[i-1][r]);
				QPointF t = transformToQT(m_path[i][r]);
				vec = t - s;
			}
			else
				vec = transformToQT(m_path[i][r]);

			QConf qconf(vec,angle);
			path_for_r.push_back(qconf);

			result_vectors[i].push_back(path_for_r);
		}
	}

	/*	If "result" is empty, the GUI assumes that a solution haven't been found */
	m_scene->setPath(result_vectors);	
}

void Planner::extract_scenario_from_gui()
{
	Point_2 ptl = transformToCGAL(m_scene->getRoomTopLeft());
	Point_2 pbr = transformToCGAL(m_scene->getRoomBottomRight());
	Point_2 pbl(ptl.x(), pbr.y());
	Point_2 ptr(pbr.x(), ptl.y());
	m_room = Room(ptl,pbr);

	QVector<QPolygonF> obstacles = m_scene->getObstacles();

	vector<QPolygonF> robots = m_scene->getRobotPolygons();
	foreach(QPolygonF qp, robots)
	{
		Polygon_2 cp;
		for (int j=0; j < qp.size(); j++)
		{
			cp.push_back(transformToCGAL(qp[j]));
		}

		m_robot_polygons.push_back(cp);
	}

	foreach(QPolygonF qp, obstacles)
	{
		Polygon_2 cp;
		for (int j=0; j < qp.size(); j++)
		{
			cp.push_back(transformToCGAL(qp[j]));
		}

		m_obstacles.push_back(cp);
	}

	Polygon_2 wall_left;
	wall_left.push_back(pbl);
	wall_left.push_back(ptl);

	Polygon_2 wall_top;
	wall_top.push_back(ptl);
	wall_top.push_back(ptr);

	Polygon_2 wall_right;
	wall_right.push_back(ptr);
	wall_right.push_back(pbr);

	Polygon_2 wall_bot;
	wall_bot.push_back(pbl);
	wall_bot.push_back(pbr);

	m_obstacles.push_back(wall_left);
	m_obstacles.push_back(wall_top);
	m_obstacles.push_back(wall_right);
	m_obstacles.push_back(wall_bot);

	QVector<QVector<QPointF> > startPositions = m_scene->getStartPositions();
	QVector<QVector<QPointF> > targetPositions = m_scene->getTargetPositions();

	m_robot_num = targetPositions.size();

	for (int g = 0; g < m_robot_num; g++)
	{
		m_start_confs.push_back(transformToCGAL(startPositions[g][0]));
		m_target_confs.push_back(transformToCGAL(targetPositions[g][0]));
	}
}